/******************************************************************************
 * Copyright (C) Ultraleap, Inc. 2011-2020.                                   *
 *                                                                            *
 * Use subject to the terms of the Apache License 2.0 available at            *
 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement           *
 * between Ultraleap and you, your company or other organization.             *
 ******************************************************************************/

using UnityEngine;

namespace Leap.Interaction.Internal.InteractionEngineUtility {
  public class GrabClassifierHeuristics {
    public static void UpdateClassifier(GrabClassifier classifier,
                                        ClassifierParameters grabParameters,
                                        ref Collider[][] collidingCandidates,
                                        ref int[] numberOfColliders,
                                        bool ignoreTemporal = false) {
      // Store actual minimum curl in case we override it with the ignoreTemporal flag.
      float tempMinCurl = grabParameters.MINIMUM_CURL;
      if (ignoreTemporal) {
        grabParameters.MINIMUM_CURL = -1f;
      }

      //For each probe (fingertip)
      for (int j = 0; j < classifier.probes.Length; j++) {
        //Calculate how extended the finger is
        float tempCurl = Vector3.Dot(classifier.probes[j].direction, (j != 0) ? classifier.handDirection : (classifier.handChirality ? 1f : -1f) * classifier.handXBasis);
        float curlVelocity = tempCurl - classifier.probes[j].prevTempCurl;
        classifier.probes[j].prevTempCurl = tempCurl;

        //Determine if this probe is intersecting an object
        bool collidingWithObject = false;
        for (int i = 0; i < numberOfColliders[j]; i++) {
          if (collidingCandidates[j][i].attachedRigidbody != null && collidingCandidates[j][i].attachedRigidbody == classifier.body) {
            collidingWithObject = true;
            break;
          }
        }

        //Nullify above findings if fingers are extended
        float conditionalMaxCurlVelocity = (classifier.isGrabbed ?
                                            grabParameters.GRABBED_MAXIMUM_CURL_VELOCITY :
                                            grabParameters.MAXIMUM_CURL_VELOCITY);
        collidingWithObject = collidingWithObject
                              && (tempCurl < grabParameters.MAXIMUM_CURL)
                              && (tempCurl > grabParameters.MINIMUM_CURL)
                              && (ignoreTemporal
                                  || curlVelocity < conditionalMaxCurlVelocity);

        //Probes go inside when they intersect, probes come out when they uncurl
        if (!classifier.probes[j].isInside) {
          classifier.probes[j].isInside = collidingWithObject;
          classifier.probes[j].curl = tempCurl + (j == 0 ? grabParameters.THUMB_STICKINESS : grabParameters.FINGER_STICKINESS);
          if (ignoreTemporal) {
            classifier.probes[j].curl = 0f + (j == 0 ? grabParameters.THUMB_STICKINESS : grabParameters.FINGER_STICKINESS);
          }
        }
        else {
          if (tempCurl > classifier.probes[j].curl) {
            classifier.probes[j].isInside = collidingWithObject;
          }
        }
      }

      //If thumb and one other finger is "inside" the object, it's a grab!
      //This is the trick!
      classifier.isThisControllerGrabbing = (classifier.probes[0].isInside && (classifier.probes[1].isInside ||
                                                                 classifier.probes[2].isInside ||
                                                                 classifier.probes[3].isInside ||
                                                                 classifier.probes[4].isInside));
      //If grabbing within 10 frames of releasing, discard grab.
      //Suppresses spurious regrabs and makes throws work better.
      if (classifier.coolDownProgress <= grabParameters.GRAB_COOLDOWN && !ignoreTemporal) {
        if (classifier.isThisControllerGrabbing) {
          classifier.isThisControllerGrabbing = false;
        }
        classifier.coolDownProgress += Time.fixedDeltaTime;
      }

      //Determine if the object is near the hand or if it's too far away
      if (classifier.isThisControllerGrabbing && !classifier.prevThisControllerGrabbing) {
        bool nearObject = false;
        numberOfColliders[5] = Physics.OverlapSphereNonAlloc(classifier.handGrabCenter, grabParameters.MAXIMUM_DISTANCE_FROM_HAND, collidingCandidates[5], grabParameters.LAYER_MASK, grabParameters.GRAB_TRIGGERS);
        for (int i = 0; i < numberOfColliders[5]; i++) {
          if (collidingCandidates[5][i].attachedRigidbody != null && collidingCandidates[5][i].attachedRigidbody == classifier.body) {
            nearObject = true;
            break;
          }
        }

        if (!nearObject) {
          classifier.isThisControllerGrabbing = false;
          classifier.probes[0].isInside = false;
        }
      }

      // Reset the minimum curl parameter if we modified it due to the ignoreTemporal
      // flag.
      if (ignoreTemporal) {
        grabParameters.MINIMUM_CURL = tempMinCurl;
      }
    }

    //Expensive collider query optimization that somehow got undone before
    public static void UpdateAllProbeColliders(Vector3[] aPositions, Vector3[] bPositions, ref Collider[][] collidingCandidates, ref int[] numberOfColliders, ClassifierParameters grabParameters) {
      for (int i = 0; i < 5; i++) {
        numberOfColliders[i] = Physics.OverlapCapsuleNonAlloc(
                                 point0:  aPositions[i],
                                 point1:  bPositions[i],
                                 radius:    i == 0 ? grabParameters.THUMBTIP_RADIUS : grabParameters.FINGERTIP_RADIUS,
                                 results:   collidingCandidates[i],
                                 layerMask: grabParameters.LAYER_MASK,
                                 queryTriggerInteraction: grabParameters.GRAB_TRIGGERS);
      }
    }

    //Per-Object Per-Hand Classifier
    public class GrabClassifier {
      public bool        isThisControllerGrabbing;
      public bool        prevThisControllerGrabbing;
      public GrabProbe[] probes = new GrabProbe[5];
      public Transform   transform;
      public Rigidbody   body;
      public bool        isGrabbed;
      public float       coolDownProgress;
      public Vector3     handGrabCenter;
      public Vector3     handDirection;
      public Vector3     handXBasis;
      public bool        handChirality;

      public GrabClassifier(GameObject behaviour) {
        probes = new GrabProbe[5];
        for(int i = 0; i<probes.Length; i++) { probes[i].prevTempCurl = 1f; }
        transform = behaviour.transform;
        body = behaviour.GetComponent<Rigidbody>();
        coolDownProgress = 0;
      }
    }

    //Per-Finger Per-Object Probe
    public struct GrabProbe {
      public Vector3 direction;
      public bool    isInside; 
      public float   curl;
      public float   prevTempCurl;
    };

    //The parameters that tune how grabbing feels
    public struct ClassifierParameters {
      /** <summary> The amount of curl hysteresis on each finger type </summary> */
      public float FINGER_STICKINESS, THUMB_STICKINESS;
      /** <summary> The minimum and maximum curl values fingers are allowed to "Grab" within </summary> */
      public float MAXIMUM_CURL, MINIMUM_CURL;
      /** <summary> The radius considered for intersection around the fingertips </summary> */
      public float FINGERTIP_RADIUS, THUMBTIP_RADIUS;
      /** <summary> The minimum amount of time between repeated grabs of a single object </summary> */
      public float GRAB_COOLDOWN;
      /** <summary> The maximum rate that the fingers are extending where grabs are considered. </summary> */
      public float MAXIMUM_CURL_VELOCITY;
      public float GRABBED_MAXIMUM_CURL_VELOCITY;
      public float MAXIMUM_DISTANCE_FROM_HAND;
      public int   LAYER_MASK;
      public QueryTriggerInteraction GRAB_TRIGGERS;

      public ClassifierParameters(float fingerStickiness = 0f, float thumbStickiness = 0.05f, float maxCurl = 0.65f, float minCurl = -0.1f, float fingerRadius = 0.01f, float thumbRadius = 0.015f, float grabCooldown = 0.2f, float maxCurlVel = 0.0f, float grabbedMaxCurlVel = -0.05f, float maxHandDist = 0.1f, int layerMask = 0, QueryTriggerInteraction queryTriggers = QueryTriggerInteraction.UseGlobal) {
        FINGER_STICKINESS             = fingerStickiness;
        THUMB_STICKINESS              = thumbStickiness;
        MAXIMUM_CURL                  = maxCurl;
        MINIMUM_CURL                  = minCurl;
        FINGERTIP_RADIUS              = fingerRadius;
        THUMBTIP_RADIUS               = thumbRadius;
        GRAB_COOLDOWN                 = grabCooldown;
        MAXIMUM_CURL_VELOCITY         = maxCurlVel;
        GRABBED_MAXIMUM_CURL_VELOCITY = grabbedMaxCurlVel;
        MAXIMUM_DISTANCE_FROM_HAND    = maxHandDist;
        LAYER_MASK                    = layerMask;
        GRAB_TRIGGERS                 = queryTriggers;
      }
    }
  }
}
